package org.lejos.example;

import lejos.nxt.*;

/**
 * Example leJOS Project with an ant build file
 *
 */
public class SensorTest {

	public static void main(String[] args) {
		System.out.println("If I ruled the world");
		Button.waitForPress();
                
            //Ultrasonic Sensor Test
            UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S4);
               while (!Button.ESCAPE.isPressed()) {
                  LCD.clear();
                  LCD.drawString(sonic.getVersion(), 0, 0);
                  LCD.drawString(sonic.getProductID(), 0, 1);
                  LCD.drawString(sonic.getSensorType(), 0, 2);
                  LCD.drawInt(sonic.getDistance(), 0, 3);
                }

                //Motor Test
                Motor.A.forward();
                Motor.B.forward();
                //Motor.C.forward();
                LCD.drawString("Forward", 0, 0);
                Button.waitForPress();
                Motor.A.reverseDirection();
                Motor.B.reverseDirection();
                //Motor.C.reverseDirection();
                LCD.drawString("Backwards", 0, 1);
                Button.waitForPress();
                Motor.A.stop();
                Motor.B.stop();
                //Motor.C.stop();

               //Touch Sensor Test
                TouchSensor touch = new TouchSensor(SensorPort.S1);
                while(touch.isPressed()){
                    System.out.println("touch");
                }

                //Light Sensor Test
                LightSensor lightSensor = new LightSensor(SensorPort.S3);
                while (true) {
                    LCD.drawInt(lightSensor.readValue(), 4, 0, 0);
                    LCD.drawInt(lightSensor.readNormalizedValue(), 4, 0, 1);
                    LCD.drawInt(SensorPort.S1.readRawValue(), 4, 0, 2);
                    LCD.drawInt(SensorPort.S1.readValue(), 4, 0, 3);
                }

	}
}
